

#include <physm.h>
#include <math.h>

#define _PHYSM_G (6.67428 * pow(10, -11))

void _physm_math_force(physm_body_t *b1, physm_body_t *b2, vm_vector *f){

  double mass_prod = b1->mass * b2->mass * (-_PHYSM_G);

#ifndef _PHYSM_PROC_x86_64
  vm_vector r;
  vm_vector r_hat;
  vm_sub(b1->loc, b2->loc, &r);
  vm_norm(r, &r_hat);
  double r_sqrd;
  vm_dot(r, r, &r_sqrd);
  vm_scale(r_hat, mass_prod / r_sqrd, f);
#else
  vm_vector r = b1->loc;
  _vm_asm_sub(&r, &b2->loc);
  _vm_asm_norm(&r, &r);
  double r_sqrd = mass_prod / _vm_asm_dot(&r, &r);
  _vm_asm_scale(&r, &r_sqrd, f);
#endif

}

int _physm_math_calc_force(physm_world_t *world, physm_body_t *body){

  PHYSM_LOCK(body);

  int body_count = physm_llist_len(world->bodies);

  int i = 0;
  for ( ; i < body_count; i++){
    physm_body_t *tmp = physm_llist_get(world->bodies, i);
    if (tmp->id == body->id) continue; // There the same body;

    /* Calc force */
    vm_vector force;
    _physm_math_force(body, tmp, &force);
#ifndef _PHYSM_PROC_x86_64
    vm_add(body->force, force, &body->force);
#else
    _vm_asm_add(&body->force, &force);
#endif    

    /* Now calculate acceleration */
#ifndef _PHYSM_PROC_x86_64
    vm_scale(force, 1 / body->mass, &body->accel);
#else
    double inv_mass = 1 / body->mass;
    _vm_asm_scale(&force, &inv_mass, &body->accel);
#endif

  }

  PHYSM_UNLOCK(body);

  return 0;

}

void _physm_math_reloc_body(physm_world_t *world, physm_body_t *body){

  PHYSM_LOCK(body);

  /* Calc velocity. */
  vm_vector accel_tau;
#ifndef _PHYSM_PROC_x86_64
  vm_scale(body->accel, world->tau, &accel_tau);
  vm_add(body->vel, accel_tau, &body->vel);
#else
  _vm_asm_scale(&body->accel, &world->tau, &accel_tau);
  _vm_asm_add(&body->vel, &accel_tau);
#endif

  /* Now calc displacement. */
  vm_vector vel_tau;
#ifndef _PHYSM_PROC_x86_64
  fprintf(stderr, "1");
  vm_scale(body->vel, world->tau, &vel_tau);
  vm_add(body->loc, vel_tau, &body->loc);
#elsif
  fprintf(stderr, "2");
  _vm_asm_scale(&body->vel, &world->tau, &vel_tau);
  _vm_asm_add(&body->loc, &vel_tau);
#endif

  PHYSM_UNLOCK(body);

}
